/*
*******************************************************************************
**
**	Copyright (C) 2011, Fujian WITSI Micro-electronic Tech. CO.,LTD.,
**	All Rights Reserved.
**
*******************************************************************************
*/
/*
*******************************************************************************
**
** File:mmu.c
**
** Brief:
**
** Logs:
**	2012.12.02		Created by tenly
** BUGREPORT:
**
*******************************************************************************
*/

/*
*******************************************************************************
**	include files
*******************************************************************************
*/
#include "config.h"
#include "init.h"
#include "iodef.h"
#include "regs.h"
#include "m14kc.h"
#include "mmu.h"

/*
*******************************************************************************
**	local referenced external function and variables
*******************************************************************************
*/

/*
*******************************************************************************
**	local referenced macro defines & type defines
*******************************************************************************
*/
#define PAGE_SHIFT	12	// PAGE_SIEZ_DEFAULT: 4K
#define UNIQUE_ENTRYHI(idx) (KSEG0_VIRTUAL_ADDR + ((idx) << (PAGE_SHIFT + 1)))

/*
*******************************************************************************
**	global variables
*******************************************************************************
*/

/*
*******************************************************************************
**	local function prototype
*******************************************************************************
*/

/*
*******************************************************************************
**	Implementation functions
*******************************************************************************
*/
void set_tlb(uint32_t page_mask,uint32_t EntryHi,uint32_t EntryLo0,uint32_t EntryLo1,uint32_t index)
{
	unsigned int old_entry;
	unsigned int old_pagemask;
/*
	printf("PageMask :0x%08x\n", page_mask);
	printf("EntryHi  :0x%08x\n", EntryHi);
	printf("EntryLo0 :0x%08x\n", EntryLo0);
	printf("EntryLo1 :0x%08x\n", EntryLo1);
	printf("Index    :0x%08x\n", index);
*/
	old_entry = read_c0_entryhi();
	old_pagemask = read_c0_pagemask();

	write_c0_index(index);
	write_c0_pagemask(page_mask);
	write_c0_entryhi(EntryHi);
	write_c0_entrylo0(EntryLo0);
	write_c0_entrylo1(EntryLo1);
	mtc0_tlbw_hazard();	// hazard!
	tlb_write_indexed();
	tlbw_use_hazard();	// hazard!

	write_c0_entryhi(old_entry);
	write_c0_pagemask(old_pagemask);
}

int mmu_map(
		uint32_t vaddr,
		uint32_t paddr,
		uint32_t psize,
		uint32_t index,
		//uint32_t asid,
		uint32_t attr)
{
	uint32_t paddr_Lo0;
	uint32_t paddr_Lo1;
	uint32_t pageMask;
	uint32_t EntryHi;
	uint32_t EntryLo0;
	uint32_t EntryLo1;

	paddr_Lo0 = paddr>>12;			// force to 4K align
	paddr_Lo1 = (paddr+psize)>>12;
	pageMask = PAGE_MASK(psize);
	EntryHi = vaddr&(~(PSIZE_4K-1));	// force to 4K align
/*	if(ASID_VALID(asid))
		EntryHi |= asid;
	else*/
		attr |= G_EN;
	attr |= V_EN;
	EntryLo0 = (((paddr_Lo0)<<6))|attr; /*EntryLO0 #PFN0|c|d|v|G|*/
	EntryLo1 = (((paddr_Lo1)<<6))|attr; /*EntryLO1 #PFN1|c|d|v|G|*/
	set_tlb(pageMask,EntryHi,EntryLo0,EntryLo1,index);
	return 0;
}

void local_flush_tlb_all(void)
{
	unsigned long old_ctx;
	int entry;
	unsigned int tlbsize;

	/* Save old context and create impossible VPN2 value */
	old_ctx = read_c0_entryhi();
	write_c0_entrylo0(0);
	write_c0_entrylo1(0);

	tlbsize = ((read_c0_config1() >> 25) & 0x3f) + 1;
	entry = read_c0_wired();

	/* Blast 'em all away. */
	while (entry < tlbsize) {
		/* Make sure all entries differ. */
		write_c0_entryhi(UNIQUE_ENTRYHI(entry));
		write_c0_index(entry);
		mtc0_tlbw_hazard();
		tlb_write_indexed();
		entry++;
	}
	tlbw_use_hazard();
	write_c0_entryhi(old_ctx);
}

static int tlb_init(void)
{
	unsigned int config = read_c0_config();
	unsigned int wired = CONFIG_TLB_WIRED;
	unsigned int tlbsize;

	if (!((config >> 7) & 3)) {
		//printf("No TLB present");
		return 0;
	}

	tlbsize = ((read_c0_config1() >> 25) & 0x3f) + 1;
	if(wired > (tlbsize-2)) wired = (tlbsize-2);

	write_c0_pagemask(PM_DEFAULT_MASK);
	write_c0_wired(wired);
	write_c0_index(wired-1);

	local_flush_tlb_all();
	return 0;
}
init_initcall(tlb_init);

/*
**=============================================================================
**	End of this file.
**	Don't insert any code after this block.
**=============================================================================
*/


